// MINI QUAD AIRFRAME

// Attitude control gains
#define KP_ROLL 0.56
#define KD_ROLL 0.24
#define KI_ROLL 0.

#define KP_PITCH 0.56
#define KD_PITCH 0.24
// ou 0.56 et 0.24
// ou 0.30 et 0.17
#define KI_PITCH 0.
#define K_PITCHofROLL 0.

//#define KP_YAW 2.0
//#define KD_YAW (0.25*KP_YAW)
#define KP_YAW 0.
#define KD_YAW 0.5
#define KI_YAW 0.

#define KD_COMMAND_PART 0.5     // for special KD implementation (between 0 and 1)

//Saturations
#define I_E_SATURATION 5.       //saturation of input of integrators (degrees)
#define I_S_SATURATION 20       //saturation of output of integrators (us)
#define COMMAND_SATURATION 80   //saturation of output of each PIDs (us)


/* ***************************************************************************************** */
#define PRINT_GCS 1     //print infos for ground station
#define PRINT_BINARY 1  //Will print binary message and suppress ASCII messages (above)

#define SOFTWARE_VER "1.74"

/* ***************************************************************************************** */
#define MIX_ROLL 0.05           // Mixing roll  (O to 1)
#define MIX_PITCH 0.05          // Mixing pitch (O to 1)
#define MIX_YAW 0.05            // Mixing yaw (O to 1)
#define MIX_THROTTLE 1          // Mixing throttle (O to 1)

#define FAILSAFE_ROLL 0         // Roll in failsafe mode
#define FAILSAFE_PITCH 0        // Pitch in failsafe mode

#define MAX_ROLL 20.            // Maximum absolute roll (in °)
#define MAX_PITCH 20.           // Maximum absolute pitch (in °)
#define MAX_YAW_SPEED 25.       // Maximum absolute yaw speed (in °/s)
#define CRUISE_SPEED 0          // Cruise speed (in m/s) (for centrifugal force estimation)

// Radio modes
#define MANUAL 0
#define FLY_BY_WIRE 1
#define TEST_MODE 2
#define NO_RADIO 3

/* ***************************************************************************************** */
// Mixing
//motor front 0
//motor right 1
//servo back 2
//servo left 3

#define SERVO_0_COMMAND (control_throttle + control_pitch + control_yaw)
#define SERVO_1_COMMAND (control_throttle - control_roll - control_yaw)
#define SERVO_2_COMMAND (control_throttle - control_pitch + control_yaw)
#define SERVO_3_COMMAND (control_throttle + control_roll - control_yaw)

//MIN_CHANN for ESC and NEUTRAL_CHANN for servos
#define SERVO_0_NEUTRAL MIN_CHANN
#define SERVO_1_NEUTRAL MIN_CHANN
#define SERVO_2_NEUTRAL MIN_CHANN
#define SERVO_3_NEUTRAL MIN_CHANN

#define TRIM_SERVO_0 0          // Trim servo 0 (in us)
#define TRIM_SERVO_1 0          // Trim servo 1 (in us)
#define TRIM_SERVO_2 0          // Trim servo 2 (in us)
#define TRIM_SERVO_3 0          // Trim servo 3 (in us)

#define OUTPUT_MAESTRO 0        // Use Pololu Maestro for servos output
#define OUTPUT_SERVOTIMER2 1    // Use ServoTimer2 for servos output (Not Implemented yet)

//Loop speed /!\ AHRS_LOOP must be a multiple of all other loops rate
#define AHRS_LOOP 100           // IMU main loop (in Hz)
#define MAGNETOMETER_LOOP 10    // IMU magnetometer loop (in Hz)
#define TELEMETRY_LOOP 20       // Telemetry loop (in Hz)
#define RADIO_LOOP 50           // Radio RX loop (in Hz)
#define CONTROL_LOOP 100        // Control (PID + servos update) loop (in Hz)

